#ifndef CTRL_H
#define CTRL_H

#include "utils.h"
#include "stdint.h"
#include "stm32f4xx.h"
#include "toolBoxScope.h"
#include <string.h>

/**
 * canId 使用情况
 * 201 ~ 204 大疆驱动板1~4普通命令控制
 * 205 ~ 208 大疆驱动板1~4普通命令控制
 */

/**
 * @brief 控制模式
 *
 */
typedef enum
{
    /* 电流环 */
    CUR_CONTROL_MODE = 1,
    /* 速度环 */
    SPEED_CONTROL_MODE,
    /* 位置环 */
    POSITION_CONTROL_MODE,
    /* 找零模式 */
    HOMING_MODE,
    SPEED_LIMIT_MODE // Robocon2021射箭云台专用
} ControlMode;

/**
 * @brief 支持控制电机名称
 *
 */
typedef enum
{
    /* DJ_3508 */
    RM_3508 = 1,
    /* DJ_2006 */
    M_2006,
    /* DJ_6020 */
    GM_6020,
    NONE = 255,
} MotorType_TypeDef;

//控制参数
#define CURRENT_MAX_3508 18.0f
#define CURRENT_MAX_2006 8.0f
#define VOLTAGE_MAX_6020 30.0f

#define VEL_MAX_3508 10000.0f
#define VEL_KP_3508 0.1f
#define VEL_KI_3508 1.0f
#define POS_KP_3508 0.11f
#define POS_KD_3508 0.01f

#define VEL_MAX_2006 10000.0f
#define VEL_KP_2006 0.05f
#define VEL_KI_2006 2.0f
#define POS_KP_2006 0.027f
#define POS_KD_2006 0.05f

#define VEL_MAX_6020 10000.0f
#define VEL_KP_6020 0.18f
#define VEL_KI_6020 1.5f
#define POS_KP_6020 0.03f
#define POS_KD_6020 0.07f

/**
 * @brief 电机返回报文信息结构体
 *
 */
typedef struct
{
    int32_t pos, posLast;

    MotorType_TypeDef type;

    int32_t vel;

    int16_t cur;

    int8_t temp;
} MotorType;

/**
 * @brief 和主控通信结构体
 *
 */
typedef struct
{
    int32_t can_statusl;
    int32_t canId;
} CommandType;

/**
 * @brief 速度环控制结构体
 *
 */
typedef struct
{
    /* 实际速度 */
    float actualSpeed;
    /* 预期速度 */
    float desireSpeed;
    /* 最大速度 */
    float maxSpeed;

    float leftLimit;

    float rightLimit;
    /* 速度环PID */
    PID_Struct speed_pid;
    /* 输出 */
    float output;
    /* 最大输出限幅 */
    float maxOutput;
} VelCtrlType;

typedef enum
{
    UP = 1, // 上升阶段标志
    DOWN,   // 下降阶段标志
    COMMON, //匀速阶段标志
} PosFlag_TypeDef;
/**
 * @brief 位置环控制结构体
 *
 */
typedef struct
{
    /* 实际位置 */
    float actulPos;
    /* 预期位置 */
    float desirePos;
    /* 位置环速度 */
    float PosVel;
    /* 位置环PID */
    PID_Struct pos_pid;

    float output; // 输出为预期速度

    int arrivalInform; // 位置环是否到达，用于主控向 DJ_Board 发信息后的反馈 command.c 中的 SetPosCtrl 对其赋值为1

    PosFlag_TypeDef arrivalFlag;
} PosCtrlType;

/**
 * @brief 电流环控制结构体
 *
 */
typedef struct
{
    float maxCur;

    float desireCur;

    float output;
} CurCtrlType;

/**
 * @brief 找零模式结构体
 *
 */
typedef struct
{
    float vel;

    float current;

    float initPos;

    int32_t cnt;

    float output;

} HomingModeType;

/**
 * @brief 电机控制结构体
 *
 */
typedef struct
{
    /* 控制模式 */
    uint32_t controlMode;

    int32_t time;

    float output;
    /* 电机状态 */
    FunctionalState status;
    /* 速度环控制结构体 */
    VelCtrlType velCtrl;
    /* 位置环控制结构体 */
    PosCtrlType posCtrl;
    /* 电流环控制结构体 */
    CurCtrlType curCtrl;
    /* 找零控制结构体 */
    HomingModeType homingMode;
    /* 电机位置周期 360°的值*/
    int32_t period;
    /* 主控通信结构体 */
    CommandType command;

} DriverType;

typedef enum
{
    CURVE_NONE = 0,   //直启
    CURVE_TRAP = 1,   //梯形曲线
    CURVE_SIGMOID = 2 // S型曲线
} SpeedCurveType;

/**
 * @brief  速度S形曲线结构体
 *
 */
typedef struct
{
    float startSpeed;         // 开始调速时的初始速度
    float currentSpeed;       // 当前速度
    float targetSpeed;        // 目标速度
    float stepSpeed;          // 加速度
    float speedMax;           // 最大速度
    float speedMin;           // 最小速度
    uint16_t aTimes;          //调速时间
    uint16_t maxTimes;        //调速跨度
    SpeedCurveType curveMode; //曲线类型
    float flexible;           // S曲线 拉伸度

} CurveObjectType;
/* ------------------------------------------------------- */

/* 电机返回报文信息结构体 */
extern MotorType motors[4];
/* 电机控制结构体*/
extern DriverType drivers[4];
/* 受控电机类型 */
extern MotorType_TypeDef motorType[4];
/* 电机控制模式 */
extern ControlMode motorControlMode[4];
/* 位置环最大速度 */
extern int maxPosVel[4];
/* 示波器显示电机ID */
extern int waveNum;
/* 展示位置环输出 */
extern float desireSpeed[4];
extern float posDelta[4];

void RM_3508_init(DriverType *driver, uint8_t mode);
void M_2006_init(DriverType *driver, uint8_t mode);
void GM_6020_init(DriverType *driver, uint8_t mode);
void DriverInit(void);
void MotorCtrl(void);
void UpdateSpeedPos(DriverType *driver, MotorType *motor);
float PosCtrl(PosCtrlType *posPid, int i);
float VelCtrl(VelCtrlType *velPid);
float HomingMode(DriverType *driver);
float MaxMinLimit(float val, float limit);
float SpeedLimitMode(VelCtrlType *velPid, float pos);

float SigmoidPosSpeed(DriverType *driver);
float SpeedMapping(DriverType *driver);
float sigmoid(float x);
void MotorVelCurve(CurveObjectType *curve);
void CalCurveSigmoid(CurveObjectType *curve);
#endif
